/*
 * G2OParse.cpp
 *
 *  Created on: Sep 8, 2016
 *      Author: ubuntu
 */

#include "G2OParse.h"
#include "../common.h"
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

static inline void G2O_parse_login(unsigned char *buf, size_t len, struct g2o_msg *msg);
static inline void G2O_parse_conf_info(unsigned char *buf, size_t len, struct g2o_msg *msg);
static inline void G2O_parse_travel_stcs(unsigned char *buf, size_t len, struct g2o_msg *msg);
static inline void G2O_parse_warn_msg(unsigned char *buf, size_t len, struct g2o_msg *msg);
static inline void G2O_parse_packed_data(unsigned char *buf, size_t len, struct g2o_msg *msg);
static inline void G2O_parse_beat_data(unsigned char *buf, size_t len, struct g2o_msg *msg);
static inline void G2O_parse_upgrade(unsigned char *buf, size_t len, struct g2o_msg *msg);

static struct g2o_gps* G2O_parse_gps(unsigned char *buf, size_t len);
static struct g2o_data_stream *G2O_parse_data_stream(unsigned char *buf, size_t len);

/* 	元征2G OBD设备消息采用 小端字节传送 
 *	消息体格式: 
 *   ------------------------------------------------------_
 *  |  帧长  | 业务ID | 协议号 |  数 据  | 时间戳 |  校验  |
 *  | 2bytes | 2bytes | 1bytes | n bytes | 4bytes | 2bytes |
 *   -----------------------------------------------------
 *  帧长: 从帧长到校验码的所有数据的长度
 */

void G2OParsePackage(unsigned char *buf, size_t len, struct g2o_msg *msg)
{
	int p = 0;
	size_t msg_len = buf[0] | buf[1] << 8; /* 帧长: 整个消息体长度 */
	p += 2;
	if (msg_len > len) {
		debug(LOG_DEBUG, "The buffer length is smaller then frame length: [%lu] [%lu]", msg_len, len);
		msg->is_valid = 0;
		return;
	} else if (msg_len == len) {
		msg->is_valid = 1;
	} else {
		msg->is_valid = 1;
		debug(LOG_DEBUG, "The buffer is enouth long to parse: [%lu] [%lu]", msg_len, len);
	}

	msg->id = buf[p] | buf[p+1] << 8;		/* 业务 ID */
	p += 2;
	msg->pro = buf[p++];					/* 协议号 */

	if (msg->id == G2O_UNKNOWN_000A || msg->id == G2O_UNKNOWN_000F)
		return;

	if (msg->id != G2O_LOGIN) {
		unsigned short vcode = buf[len-1] | buf[len-2] << 8; /* 校验码: 2字节，位于消息队尾 */
		unsigned short crc = crc16(0xFFFF, buf, len-2); /* 采用crc16 校验 */
		if (vcode != crc) {
			debug(LOG_DEBUG, "There is a problem in crc16: %04X %04X", vcode, crc);
		}
	}


	size_t pos = len - 6;					/* 消息数据尾，时间戳的起始位置 */
	size_t nLen = pos - p;					/* 消息数据总长度 */

	unsigned char *dBuf = buf+p; 			/* 消息数据起始地址 */

	msg->ts = buf[pos] | buf[pos+1] << 8 | buf[pos+2] << 16 | buf[pos+3] << 24;											/* 时间戳 */

	switch (msg->id) {
	case G2O_LOGIN:
		G2O_parse_login(dBuf, nLen, msg);
		break;
	case G2O_CONFIG_INFO:
		G2O_parse_conf_info(dBuf, nLen, msg);
		break;
	case G2O_WARN_MSG:
		G2O_parse_warn_msg(dBuf, nLen, msg);
		break;
	case G2O_TRAVEL_STCS:
		G2O_parse_travel_stcs(dBuf, nLen, msg);
		break;
	case G2O_PACKED_DATA:
		G2O_parse_packed_data(dBuf, nLen, msg);
		break;
	case G2O_BEAT_DATA:
		G2O_parse_beat_data(dBuf, nLen, msg);
		break;
	case G2O_UPGRADE:
		G2O_parse_upgrade(dBuf, nLen, msg);
		break;
	case G2O_UNKNOWN_000A:
		msg->is_valid = 0;
		break;
	case G2O_UNKNOWN_000F:
		return;
	default:
		debug(LOG_DEBUG, "不存在的类型: %04X", msg->id);
		break;
	}
}

/*		登录请求消息体
 *	 ----------------------------------------------------------------------------
 *	| 开发者代码 | 设备序列号 | 设备密钥 | 设备IMEI码 | SIM卡号 | 设备软件版本号 |
 *  |  2 bytes   |  12 bytes  | 32 bytes |  15 bytes  | 20bytes |  DPU string    |
 *   ----------------------------------------------------------------------------
 *	DPU string: < 1 byte 长度字节 > + < 字符串 >
 *	所有字符串均为ASCII码
 */

static inline void G2O_parse_login(unsigned char *buf, size_t len, struct g2o_msg *msg)
{
	if (len < 82) {
		msg->is_valid = 0;
		debug(LOG_DEBUG, "The length of login request is smaller then 82: %lu", len);
		return;
	}
	struct g2o_login *login = (struct g2o_login *)malloc(sizeof(struct g2o_login));
	size_t pos = 0;
	if (!login) {
		msg->is_valid = 0;
		debug(LOG_DEBUG, "I'm sorry, malloc is failed, so not parse again");
		return;
	}
	msg->login = login;
	memset(login, 0, sizeof(struct g2o_login));
	memcpy(login->dp_code, buf+pos, 2);					/* 开发者代码: 2  */
	pos += 2;
	memcpy(login->obdid, buf+pos, 12);					/* 设备序列号: 12 */
	pos += 12;
	memcpy(login->dev_key, buf+pos, 32);				/* 设备密钥: 32 */
	pos += 32;
	memcpy(login->dev_imei, buf+pos, 15);				/* 设备IMEI: 15*/
	pos += 15;
	memcpy(login->dev_sim, buf+pos, 20);				/* SIM 卡号 */
	pos += 20;
	login->dpu_len = buf[pos++];						/* 设备软件 dup string*/
	if (len >= login->dpu_len + pos) {
		login->dev_version = (char *)malloc(login->dpu_len+1);
		if (login->dev_version) {
			memcpy(login->dev_version, buf+pos, login->dpu_len);
			login->dev_version[login->dpu_len] = 0;
		}
	}
}

static inline void G2O_parse_conf_info(unsigned char *buf, size_t len, struct g2o_msg *msg)
{
	if (len < 1) {
		debug(LOG_DEBUG, "配置车型信息长度不足");
		msg->is_valid = 0;
		return;
	}
	msg->cfg_type = buf[0];
	switch (msg->cfg_type) {
	case 0x01:
		break;
	case 0x02:
		if (len >= 5) {
			msg->car_typeid = buf[1] | buf[2] << 8 | buf[3] << 16 | buf[4] << 24;
		} else {
			msg->is_valid = 0;
			return;
		}
		break;
	default:
		return;
	}
	msg->is_valid = 1;
}

static inline void G2O_parse_warn_msg(unsigned char *buf, size_t len, struct g2o_msg *msg)
{
	if (len < 2) {
		debug(LOG_DEBUG, "报警信息长度不足");
		msg->is_valid = 0;
		return;
	}
	size_t i = 0;
	struct g2o_warn *warn = (struct g2o_warn *)malloc(sizeof(struct g2o_warn));
	if (!warn) {
		debug(LOG_DEBUG, "报警信息处理内存分配失败");
		msg->is_valid = 0;
		return;
	}
	msg->warn = warn;
	warn->id = buf[i] << 8 | buf[i+1];		/* 报警消息ID */
	i += 2;
	switch (warn->id) {
	case G2O_WARN_TYPE_FAST:
	case G2O_WARN_TYPE_DESC:
	case G2O_WARN_TYPE_VIBRATION:
		warn->warn_len = 0;
		warn->warn_msg = NULL;
		break;
	case G2O_WARN_TYPE_TIRED:
	case G2O_WARN_TYPE_IDLE:
	case G2O_WARN_TYPE_STARTUP:
	case G2O_WARN_TYPE_SHARP_TURN:
	case G2O_WARN_TYPE_INSERT_PULLED:
	default:
		{
			if (i+1 >= len) {
				msg->is_valid = 0;
				free(warn);
				debug(LOG_ERR, "报警消息内容长度不足");
				return;
			}
			warn->warn_len = buf[i++];
			if (warn->warn_len == 0) {
				warn->warn_msg = NULL;
				break;
			}
			if (warn->warn_len + i > len) {
				debug(LOG_ERR, "报警消息上报长度过大: [%lu] [%lu]", warn->warn_len+i, len);
				msg->is_valid = 0;
				free(warn);
				return;
			}
			warn->warn_msg = (char *)malloc(warn->warn_len+1);
			if (warn->warn_msg) {
				memcpy(warn->warn_msg, &buf[i], warn->warn_len);
				warn->warn_msg[warn->warn_len] = 0;
			} else {
				debug(LOG_DEBUG, "报警消息内容内存分配失败");
				free(warn);
				msg->is_valid = 0;
				return;
			}
		}
		break;
	}
	msg->is_valid = 1;
}

static inline void G2O_parse_travel_stcs(unsigned char *buf, size_t len, struct g2o_msg *msg)
{
	if (len < 9) {
		msg->is_valid = 0;
		debug(LOG_DEBUG, "行程长度不足");
		return;
	}
	size_t i = 0;
	struct g2o_travel *travel = (struct g2o_travel *)malloc(sizeof(struct g2o_travel));
	if (!travel) {
		debug(LOG_DEBUG, "行程消息内存分配失败");
		msg->is_valid = 0;
		return;
	}
	msg->travel = travel;
	travel->type = buf[i++];
	switch (travel->type) {
	case G2O_TRAVEL_TYPE_START:
		{
			travel->number = buf[i] | buf[i+1] << 8 | buf[i+2] << 16 | buf[i+3] << 24;
			i += 4;
			travel->ts = buf[i] | buf[i+1] << 8 | buf[i+2] << 16 | buf[i+3] << 24;
		}
		break;
	case G2O_TRAVEL_TYPE_FINISH:
		{
			if (i + 50 > len) {
				debug(LOG_DEBUG, "行程结束长度不符合要求: [%lu] [%lu]", i+50, len);
				goto data_failed;
			}
			travel->number = buf[i] | buf[i+1] << 8 | buf[i+2] << 16 | buf[i+3] << 24;
			i += 4;
			travel->ts = buf[i] | buf[i+1] << 8 | buf[i+2] << 16 | buf[i+3] << 24;
			i += 4;
			/* 开始GPS 与 结束 GPS 不知道是个什么东西 ,文档上一共说有32个字节 */
			travel->lat_start = strtodouble(&buf[i]);
			i += 8;
			travel->lng_start = strtodouble(&buf[i]);
			i += 8;
			travel->lat_end = strtodouble(&buf[i]);
			i += 8;
			travel->lng_end = strtodouble(&buf[i]);
			i += 8;
			travel->idling_cnt = buf[i] | buf[i+1] << 8;
			i += 2;
			travel->idling_tm = buf[i] | buf[i+1] << 8;
			i += 2;
			travel->mileage = buf[i] | buf[i+1] << 8 | buf[i+2] << 16 | buf[i+3] << 24;
			i += 4;
			travel->oil = buf[i] | buf[i+1] << 8;
		}
		break;
	case G2O_TRAVEL_TYPE_GPS_START:
		{
			travel->number = buf[i] | buf[i+1] << 8 | buf[i+2] << 16 | buf[i+3] << 24;
			i += 4;
			travel->ts = buf[i] | buf[i+1] << 8 | buf[i+2] << 16 | buf[i+3] << 24;
		}
		break;
	case G2O_TRAVEL_TYPE_GPS_FINISH:
		{
			if (i + 42 < len) {
				debug(LOG_DEBUG, "GPS行程结束数据长度不足: [%lu] [%lu]", i+42, len);
				goto data_failed;
			}
			travel->number = buf[i] | buf[i+1] << 8 | buf[i+2] << 16 | buf[i+3] << 24;
			i += 4;
			travel->ts = buf[i] | buf[i+1] << 8 | buf[i+2] << 16 | buf[i+3] << 24;
			i += 4;
			/* 开始GPS 与 结束 GPS 共20字节，目前不知道是个什么鬼 */
			i += 20;
			travel->idling_cnt = buf[i] | buf[i+1] << 8;
			i += 2;
			travel->idling_tm = buf[i] | buf[i+1] << 8;
			i += 2;
			travel->mileage = buf[i] | buf[i+1] << 8 | buf[i+2] << 16 | buf[i+3] << 24;
			i += 4;
			travel->oil = buf[i] | buf[i+1] << 8;
			i += 2;
			travel->ts_gps = buf[i] | buf[i+1] << 8 | buf[i+2] << 16 | buf[i+3] << 24;
		}
		break;
	default:
		break;
	}
	msg->is_valid = 1;
	return;

data_failed:
	msg->is_valid = 0;
	free(travel);
	msg->travel = NULL;
}

static inline void G2O_parse_packed_data(unsigned char *buf, size_t len, struct g2o_msg *msg)
{
	size_t i = 0, n = 0;
	size_t pos = 0;
	size_t blen = 0;
	unsigned short id = 0;
	unsigned char pro = 0;
	msg->gps_cnt = 0;
	msg->gps = NULL;
	msg->gps_tail = NULL;
	msg->stm_cnt = 0;
	msg->stream = NULL;
	msg->stream_tail = NULL;
	msg->is_valid = 1;
	while (i+2 < len) {
		n = pos = i;
		blen = buf[n] | buf[n+1] << 8;
		n += 2;
		if (blen <= 11) {
			debug(LOG_DEBUG, "打包数据内长度不足");
			break;
		}

		id = buf[n] | buf[n+1] << 8;
		n += 2;
		pro = buf[n++];

		switch (id) {
		case G2O_GPSDATA:
			{
				struct g2o_gps *gps = G2O_parse_gps(buf+pos, blen);
				if (!gps)
					continue;
				if (msg->gps_tail) {
					msg->gps_tail->next = gps;
					msg->gps_tail = gps;
				} else {
					msg->gps = gps;
					msg->gps_tail = gps;
				}
				msg->gps_cnt++;
			}
			break;
		case G2O_DATA_STREAM:
			{
				struct g2o_data_stream *stream = G2O_parse_data_stream(buf+pos, blen);
				if (!stream)
					continue;
				if (msg->stream_tail) {
					msg->stream_tail->next = stream;
					msg->stream_tail = stream;
				} else {
					msg->stream = stream;
					msg->stream_tail = stream;
				}
				msg->stm_cnt++;
			}
			break;
		default:
			i += blen;
			continue;
		}

		i += blen;
	}
}

/*		GPS 数据
 *	 --------------------------------------------------------------------
 *  | 定位模式 | 方 向 | 速 度 | 高 度 | 经 度 | 纬 度 | 时间戳 | 检验码 |
 *  |  1 byte  | 2byte | 2byte | 2byte | 8byte | 8byte | 4bytes | 2bytes |
 *   --------------------------------------------------------------------
 */

static struct g2o_gps *
G2O_parse_gps(unsigned char *buf, size_t len)
{
	if (len < 34) {
		debug(LOG_DEBUG, "GPS长度不符合要求: [%lu]", len);
		return NULL;
	}
	struct g2o_gps *gps = (struct g2o_gps *)malloc(sizeof(struct g2o_gps));
	if (!gps) {
		debug(LOG_DEBUG, "分配GPS数据包时内存分配失败");
		return  NULL;
	}
	gps->next = NULL;

	/* 应该先校验，这里就算了 */

	size_t pos = 5;							/* 去掉消息头 */
	gps->position_mode = buf[pos++];
	gps->direction = buf[pos] | buf[pos+1] << 8;
	pos += 2;
	gps->speed = buf[pos] | buf[pos+1] << 8;
	pos += 2;
	gps->height = buf[pos] | buf[pos+1] << 8;
	pos += 2;

	gps->longitude = strtodouble(buf+pos);
	pos += 8;
	gps->latitude = strtodouble(buf+pos);
	pos += 8;

	/* 未知变量 */
	pos += 2;

	gps->ts = buf[pos] | buf[pos+1] << 8 | buf[pos+2] << 16 | buf[pos+3] << 24;

	return gps;
}

/********************************************************************
 *			数据流: 0x0004
 *	 -------------------------------------------------------
 *  | 码表长度 | 码表(变长)  | 数值(变长): 据分析，数据有限 |
 *  |  1 byte  | 通常9 bytes |	好多个字节					|
 *	 -------------------------------------------------------
 *
 ********************************************************************/

static struct g2o_data_stream *
G2O_parse_data_stream(unsigned char *buf, size_t len)
{
	struct g2o_data_stream *stream = (struct g2o_data_stream *)malloc(sizeof(struct g2o_data_stream));
	size_t pos = 5;
	size_t mark_pos = 0;
	size_t dpos = 0;
	if (!stream) {
		debug(LOG_DEBUG, "数据流消息分配内存失败");
		return NULL;
	}
	stream->next = NULL;
	stream->table_len = buf[pos++];
	if (stream->table_len + pos > len) {
		debug(LOG_DEBUG, "数据流长度不合规: [%d] [%lu]", stream->table_len+pos, len);
		free(stream);
		return NULL;
	}
	int mark_size = sizeof(stream->mark)/sizeof(stream->mark[0]);
	mark_pos = pos; /* 码表起始位置 */
	if (stream->table_len > mark_size) {
		free(stream);
		debug(LOG_DEBUG, "数据流中不存在这么长的码表: %d", stream->table_len);
		return NULL;
	}
	for (int i = 0; i < mark_size; i++) {
		stream->mark[i] = -1;
	}
	
	dpos = mark_pos + stream->table_len; 			/* 数据起始位置 */
	int n = 0;
	if (buf[mark_pos+5] & GOLO_BIT7) {				/* 42: 用油量 */
		stream->oil = buf[dpos] << 16 | buf[dpos+1] << 8 | buf[dpos+2];
		dpos += 3;
		stream->mark[n++] = 42;
	} else {
		stream->oil = 0xFFFFFF;
	}

	if (buf[mark_pos+5] & GOLO_BIT6) {				/* 43: 水温 */
		stream->water_temp = buf[dpos++] - 40;
		stream->mark[n++] = 43;
	} else {
		stream->water_temp = 255;
	}

	if (buf[mark_pos+5] & GOLO_BIT5) {				/* 44: 发动机进气温度 */
		stream->air_temp = buf[dpos++] - 40;
		stream->mark[n++] = 44; 
	} else {
		stream->air_temp = 255;
	}

	if (buf[mark_pos+5] & GOLO_BIT3) {				/* 46: 电压 */ 
		stream->voltage = buf[dpos] << 8 | buf[dpos+1];
		dpos += 2;
		stream->mark[n++] = 46;
	} else {
		stream->voltage = 255;
	}


	if (stream->table_len < 7) {
		goto data_time;
	} 

	if (buf[mark_pos+6] & GOLO_BIT6) { 				/* 51: 车速 */
		stream->car_speed = buf[dpos++];
		stream->mark[n++] = 51;
	} else {
		stream->car_speed = 0xFF;
	}

	if (buf[mark_pos+6] & GOLO_BIT5) { 				/* 52: 转速 */
		stream->engine_speed = buf[dpos] << 8 | buf[dpos+1];
		dpos += 2;
		stream->mark[n++] = 52;
	} else {
		stream->engine_speed = 0xFFFF;
	}

	if (buf[mark_pos+6] & GOLO_BIT3) {				/* 54: 瞬时油耗, 单位L/km */
		stream->instant_fuel_km = buf[dpos] << 8 | buf[dpos+1];
		dpos += 2;
		stream->mark[n++] = 54;
	} else {
		stream->instant_fuel_km = 0xFFFF;
	}
	if (buf[mark_pos+6] & GOLO_BIT2) {				/* 55: 瞬时油耗, 单位L/H */
		stream->instant_fuel_h = buf[dpos] << 8 | buf[dpos+1];
		dpos += 2;
		stream->mark[n++] = 55;
	} else {
		stream->instant_fuel_h = 0xFFFF;
	}

	if (stream->table_len < 8)
		goto data_time;

	if (buf[mark_pos+7] & GOLO_BIT8) { 					/* 57: 空气流量  */
		stream->air_flow = buf[dpos] << 8 | buf[dpos+1];
		dpos += 2;
		stream->mark[n++] = 57;
	} else {
		stream->air_flow = 0xFFFF;
	}
	if (buf[mark_pos+7] & GOLO_BIT7) {					/* 58: 进气歧管绝对压力 */
		stream->press = buf[dpos++];
		stream->mark[n++] = 58;
	} else {
		stream->press = 0xFF;
	}

	if (buf[mark_pos+7] & GOLO_BIT1) { 					/* 64: 滤波后剩余油量 */
		stream->remain_fuel = buf[dpos++];
		stream->mark[n++] = 64;
	} else {
		stream->remain_fuel = 0xFF;
	}

	if (stream->table_len < 9) {
		goto data_time;
	}

	if (buf[mark_pos+8] & GOLO_BIT8) { 					/* 65: 累计里程 */
		stream->mileage = buf[dpos] << 16 | buf[dpos+1] << 8 | buf[dpos+2];
		dpos += 3;
		stream->mark[n++] = 65;
	} else {
		stream->mileage = 0xFFFFFF;
	}
	if (buf[mark_pos+8] & GOLO_BIT7) { 					/* 66: 里程ID */
		stream->mileage_id = buf[dpos] << 16 | buf[dpos+1] << 8 | buf[dpos+2];
		dpos += 3;
		stream->mark[n++] = 66;
	} else {
		stream->mileage_id = 0xFFFFFF;
	}

data_time:
	if (dpos + 4 <= len) {
		stream->ts = buf[dpos] | buf[dpos+1] << 8 | buf[dpos+2] << 16 | buf[dpos+3] << 24;
	} else {
		stream->ts = 0;
	}

	return stream;
}

static inline void G2O_parse_beat_data(unsigned char *buf, size_t len, struct g2o_msg *msg)
{
	if (len < 1) {
		msg->is_valid = 0;
		return;
	}

	msg->gsm_sig_inensity = buf[0];
}

static inline void G2O_parse_upgrade(unsigned char *buf, size_t len, struct g2o_msg *msg)
{
	if (len <= 1) {
		msg->is_valid = 0;
		return;
	}

	struct g2o_upgrade *upgrade = (struct g2o_upgrade *)malloc(sizeof(struct g2o_upgrade));
	if (!upgrade) {
		return;
	}
	msg->upgrade = upgrade;
	size_t n = 0;
	upgrade->type = buf[n++];
	upgrade->vsn_len = buf[n++];
	if (upgrade->vsn_len + n > len) {
		free(upgrade);
		msg->is_valid = 0;
		return;
	}
	if (upgrade->vsn_len >= sizeof(upgrade->version)) {
		free(upgrade);
		msg->is_valid = 0;
		return;
	}

	msg->is_valid = 1;
	memcpy(upgrade->version, &buf[n], len - n);
	upgrade->version[upgrade->vsn_len] = 0;
}


void G2O_free_msg(struct g2o_msg *msg)
{
	if (msg->is_valid == 0)
		return;
	switch (msg->id) {
	case G2O_LOGIN:
		if (msg->login) {
			if (msg->login->dev_version)
				free(msg->login->dev_version);
			free(msg->login);
		}
		break;
	case G2O_CONFIG_INFO:
		break;
	case G2O_WARN_MSG:
		if (msg->warn) {
			if (msg->warn->warn_msg)
				free(msg->warn->warn_msg);
			free(msg->warn);
		}
		break;
	case G2O_TRAVEL_STCS:
		if (msg->travel)
			free(msg->travel);
		break;
	case G2O_PACKED_DATA:
		{
			if (msg->gps_cnt > 0) {
				struct g2o_gps *tmp, *gps;
				for (gps = msg->gps; gps ; gps = tmp) {
					tmp = gps->next;
					free(gps);
				}
			}
			if (msg->stm_cnt > 0) {
				struct g2o_data_stream *tmp, *stream;
				for (stream = msg->stream; stream; stream = tmp) {
					tmp = stream->next;
					free(stream);
				}
			}
		}
		break;
	case G2O_UPGRADE:
		if (msg->upgrade)
			free(msg->upgrade);
		break;
	}
}
